#include<ros/ros.h>
#include<serial/serial.h>
#include<geometry_msgs/Twist.h>
#include<sensor_msgs/Joy.h>
#include<iostream>
#include <fcntl.h>
//线程通讯
#include <csignal> 
#include <ecl/threads.hpp>

#define TX_MAX 13
using namespace std;

uint8_t txd[TX_MAX] = {0x5A, 0xA5, 0x0D ,0x02 ,0x00 ,0x00 ,0x00 ,0x00 ,0x00 ,0x00 ,0x00 ,0x00 };
std::string _port;
serial::Serial ser;
using  namespace std ;

class chassis_driver
{
private:
//私有属性
    /* data */
    void callback(const geometry_msgs::Twist::ConstPtr & twist);
    ros::Subscriber sub;
    ros::Publisher pub;
    ros::NodeHandle n;
    geometry_msgs::Twist twist;
    //线程通讯
    ecl::Thread thread_;
    int loop_rate_;
    void thread_func();

public:
//公共属性
    chassis_driver();
    //线程通讯
    void loop();
};
//类的构造函数
chassis_driver::chassis_driver()
{
    sub = n.subscribe<geometry_msgs::Twist>("/zeus_speed",10,&chassis_driver::callback,this);
    n.getParam("chassis_driver/port",_port);
    loop_rate_ = 10;
    thread_.start(ecl::generateFunctionObject(&chassis_driver::thread_func,*this));

    try
    {
        /* code */
        ser.setPort(_port.c_str());
        ser.setBaudrate(115200);
        serial::Timeout to = serial::Timeout::simpleTimeout(5);
        ser.setTimeout(to);
        ser.open();
    }
    catch(serial::IOException& e)
    {
        std::cerr << e.what() << '\n';
        ROS_ERROR_STREAM("Ubable to open port");
        return;
    }
    while(ros::ok())
    {
        ros::spinOnce();
        ros::Rate loop_rate(10);
        loop_rate.sleep();
    }
    if(ser.isOpen())
    {
        ser.close();
    }
}
// chassis_driver::~chassis_driver()
// {
//     ecl::Thread::Thread();
// }
//回环函数接收  geometry_msgs::Twist话题
void chassis_driver::callback(const geometry_msgs::Twist::ConstPtr & twist)
{
    float linear_vel_x_min = (twist->linear.x)* 60;
	float angular_rad_z_min = (twist->angular.z) * 60;
	float tangential_vel = angular_rad_z_min * 0.445/2;
    uint32_t v = ((linear_vel_x_min + tangential_vel) / (0.165 * 3.1415926));
    uint32_t r = -((linear_vel_x_min - tangential_vel) / (0.165 * 3.1415926));
    
    ROS_INFO("linear.x:[%f],angular.z:[%f]",twist->linear.x,twist->angular.z);
    txd[0] = 0x5A;
    txd[1] = 0xA5;
    txd[2] = 0x0D;
    txd[3] = 0x02;

    txd[4] = (uint8_t)r;
    txd[5] = (uint8_t)(r >> 8);
    txd[6] = (uint8_t)(r >> 16);
    txd[7] = (uint8_t)(r >> 24);

    txd[8] = (uint8_t)v;
    txd[9] = (uint8_t)(v >> 8);
    txd[10] = (uint8_t)(v >> 16);
    txd[11] = (uint8_t)(v >> 24);

    int cs;
    cs = (txd[0]+txd[1]+txd[2]+txd[3]+txd[4]+txd[5]+txd[6]+txd[7]+txd[8]+txd[9]+txd[10]+txd[11]);
    txd[12] = cs;
    ser.write(txd,TX_MAX);
    
}
void chassis_driver::thread_func()
{
    ros::Rate rate(loop_rate_);
    while(ros::ok())
    {
	//printf("while...\n");
        rate.sleep();
    }
}

void chassis_driver::loop()
{
   ros::spin();
}

boost::shared_ptr<chassis_driver> node_;
//主函数
int main(int argc,char **argv)
{
    ros::init(argc,argv,"chassis_driver");
    ros::NodeHandle nh;

    ROS_INFO("chassis_driver start!!!");
    node_.reset(new chassis_driver());
    node_->loop();
    ROS_INFO( "chassis_driver stop!!!");
    //chassis_driver chassis_driverlog; 
    return 0 ;  
}

